#include "acuProgram.h"
#include "math.h"
#include "add.h"
void  CountAzComEr() {
	double x,y ;

	x = Az_axis.Angle ;
	y = Az_axis.Cmd_angle ;

	Az_axis.Angle_er = y-x ;

}

// PID 过程  x
double TAcuMainForm::PID_X_Process(double KP, double KI, double KD) {
	double m_K;
	static double ek1=0.0,ek2=0.0;
	static double az_pk;
	double temp,az_yn1, az1;

	CountAzComEr();
	if(!Az_axis.Motor_break) 
	{

		az_ek = 13 * Az_axis.Angle_er ;  //角度放大

		az_yn1 = az_ek;

		az_pk = KP*(az_ek-ek1)+KI*az_ek+KD*(az_ek-2.*ek1+ek2);
		ek2 = ek1;
		ek1 = az_ek;
		az_ek = az_pk;

		temp= PID_Acc_Max_Az ;//0.3125/0.3 * 0.3;
		if (az_ek>temp) az_ek = temp;
		if (az_ek<-temp) az_ek= -temp;

		az_pid += az_ek;
		temp =13 * 0.3;//m_posloopRec.Az_Speed_MAX ;
		if (fabs(az_yn1)<1.5*temp && fabs(az_yn1)>temp/10.) {
			if (fabs(az_pid)>11.)
				az_pid = az_pid/1.5*fabs(az_yn1)/temp;
		}
//================================================
//================================================
		if (az_pid > 10)az_pid = 10;
		if (az_pid <-10 )az_pid = -10;

		pid_x_out = az_pid;

	} else {
		error_x_last    = 0;
		K_x_SUM         = 0;
		pid_x_out_last  = 0;
		pid_x_out       = 0;

		ek1=0.0;
		ek2=0.0;
		az_ek = 0.0;
		az_pid=0.0;
	}
	return pid_x_out ;

}
 // PID 过程  y
double TAcuMainForm::PID_Y_Process(double KP, double KI, double KD)
 {
 double m_K;
    static double ek1=0.0,ek2=0.0;
    static double el_pk;
    double temp,el_yn1;

    El_axis.Angle_er = ( El_axis.Cmd_angle - El_axis.Angle );
if(!El_axis.Motor_break)
{
    el_ek = 13 * ( El_axis.Cmd_angle - El_axis.Angle );  //角度转速度电压
    el_yn1 = el_ek;

    el_pk = KP*(el_ek-ek1)+KI*el_ek+KD*(el_ek-2.*ek1+ek2);
    ek2 = ek1;
    ek1 = el_ek;
    el_ek = el_pk;

    temp= PID_Acc_Max_El ; //0.3125/0.3* 0.3;
    if (el_ek>temp) el_ek = temp;
    if (el_ek<-temp) el_ek= -temp;

    el_pid += el_ek;
    temp = 0.3 *13;
    if (fabs(el_yn1)<1.5*temp && fabs(el_yn1)>temp/10.)
    {
        if (fabs(el_pid)>11.)
        el_pid = el_pid/1.5*fabs(el_yn1)/temp;
    }

    if (el_pid > 10)el_pid =10;if (el_pid <-10 )el_pid = -10;

    pid_y_out = el_pid;  
}
else
{
    error_y_last    = 0;
    K_y_SUM         = 0;
    pid_y_out_last  = 0;
    pid_y_out       = 0;

    ek1=0.0;
    ek2=0.0;  
    el_ek =0.0;
    el_pid=0.0;
}

    return pid_y_out ;

}

void  TAcuMainForm::PID_X_Count(double Time_sample) {                // PID 系数计
	x_K[0] = m_posloopRec.x_KP;//* ( Time_sample/m_posloopRec.x_KI + 1 ) ;
    x_K[1] = m_posloopRec.x_KI ;
    x_K[2] = m_posloopRec.x_KD;

    PID_X.KP = x_K[0] ;//m_posloopRec.x_KP* ( Time_sample/m_posloopRec.x_KI + 1 ) ;
    PID_X.KI = -m_posloopRec.x_KP ;
    PID_X.KD = 0;

    PID_X.AtoV = PID_AngleToSpeed;
    PID_X.a_max = PID_MAX_Speed_Angle ;

    PID_Acc_Max_Az = m_posloopRec.Az_Conversion ;    //最大积分量

}



void TAcuMainForm::PID_Y_Count(double Time_sample) {                // PID 系数计算

y_K[0] = m_posloopRec.y_KP ;//* ( Time_sample/m_posloopRec.y_KI  + 1 );
    y_K[1] = m_posloopRec.y_KI ;
    y_K[2] = m_posloopRec.y_KD ;

    PID_Y.KP = y_K[0];//m_posloopRec.y_KP * ( Time_sample/m_posloopRec.y_KI  + 1 );
    PID_Y.KI = -m_posloopRec.y_KP ;
    PID_Y.KD = 0;

    PID_Acc_Max_El = m_posloopRec.El_Conversion ;    //最大积分量

}

void  TAcuMainForm::MainTimerCycle()         // 主循环
{
	x_K[0]=1;
	x_K[1]=2;
	x_K[2]=3;
    double result=   PID_X_Process(x_K[0],x_K[1],x_K[2]);
    // PID_Y_Process(y_K[0],y_K[1],y_K[2]);
    printf("result is %f\r\n",result);
    

}
void  TAcuMainForm::Init_Data()
{
//=========   PID 常量计算  =============
    PID_X_Count( SYSTEM_TIMER/1000.0 );
    PID_Y_Count( SYSTEM_TIMER/1000.0 );
    m_posloopRec.x_KP=1;
    m_posloopRec.x_KI=2;
    m_posloopRec.x_KD=3;

}


